#include <LitStdAfx.h>
#include "LitPhysicsBody.h"
#include "LitPhysicsBodyState.h"
#include "LitPhysicsWorld.h"

#include <LitSceneManager.h>
#include <LitCore.h>

namespace Lit
{
	namespace Physics
	{
		BodySerializer::BodySerializer( ){}
		BodySerializer::~BodySerializer( ){}
		
		Body* BodySerializer::importFile( World * world, const std::string &filename )
		{
			std::ifstream f( filename.c_str(), std::ios::in );
			if( !f.good() ) throw Lit::FileMissing( filename, EXC_LOC );
			
			std::string name;
			std::string mesh;
			std::string mass;
			std::string shape;
			std::string id;
			std::string dyn;
			std::string offset;
			std::string scale;
			std::string orient;

			while( !f.eof() )
			{
				std::string word, val;
				f >> word;
				if( word == "name" )  f >> name;
				else if( word == "mesh" ) f >> mesh;
				else if( word == "mass" ) f >> mass;
				else if( word == "shape" ) f >> shape;
				else if( word == "id" ) f >> id;
				else if( word == "dynamic" ) f >> dyn;
				else if( word == "offset" ) f >> dyn;
				else if( word == "scale" ) f >> dyn;
				else if( word == "orientation" ) f >> dyn;
			}
			
			if( name.empty() ) throw Lit::MissingParameter( "name", EXC_LOC );
			if( mesh.empty() ) throw Lit::MissingParameter( "mesh", EXC_LOC );
			if( mass.empty() ) throw Lit::MissingParameter( "mass", EXC_LOC );
			if( shape.empty() ) throw Lit::MissingParameter( "shape", EXC_LOC );
			if( id.empty() ) throw Lit::MissingParameter( "id", EXC_LOC );
			if( dyn.empty() ) throw Lit::MissingParameter( "dynamic", EXC_LOC );
			if( offset.empty() ) throw Lit::MissingParameter( "offset", EXC_LOC );
			if( scale.empty() ) throw Lit::MissingParameter( "scale", EXC_LOC );
			if( orient.empty() ) throw Lit::MissingParameter( "orientation", EXC_LOC );

			ShapeType shap;
			if( shape == trShapeType( ST_BOX ) ) shap = ST_BOX;
			else if( shape == trShapeType( ST_PLANE ) ) shap = ST_PLANE;
			else if( shape == trShapeType( ST_SPHERE ) ) shap = ST_SPHERE;
			else shap = ST_NULL;

			if( Ogre::StringConverter::parseBool(dyn) )
			{
				return *Lit::Root::SceneManager()->createDynamicBody( 
					world, 
					mesh, 
					Ogre::StringConverter::parseReal( mass ), 
					shap,
					NULL,
					NULL,
					Ogre::StringConverter::parseVector3(offset),
					Ogre::StringConverter::parseVector3(scale),
					Ogre::StringConverter::parseQuaternion(orient) );
			}
			else
			{
				return *Lit::Root::SceneManager()->createStaticBody( 
					world, 
					mesh, 
					shap,
					NULL,
					NULL,
					Ogre::StringConverter::parseVector3(offset),
					Ogre::StringConverter::parseVector3(scale),
					Ogre::StringConverter::parseQuaternion(orient) );
			}
			// Never reached !
			return NULL;
		}
		void BodySerializer::exportFile( Body * body, const std::string &filename )
		{
			std::ofstream of(filename.c_str(), std::ios::out);
			if( !of.good() ) throw Lit::IOError( filename, "Cannot write to file", EXC_LOC );
			of << std::boolalpha;
			of << "name " << body->getName() << std::endl;
			of << "dynamic " << !body->isStatic() << std::endl;
			of << "mesh " << body->getEntity()->getMeshName() << std::endl;
			of << "mass " << body->getMass() << std::endl;
			of << "shape " << Lit::Physics::trShapeType(body->getShape()->shapeType()) << std::endl;
			of << "id " << body->getShape()->shapeId() << std::endl;
			of << "offset " << body->getShape()->initialMatrix().getPosition() << std::endl;
			of << "scale " << body->getEntity()->getScale() << std::endl;
			of << "orientation " << body->getShape()->initialMatrix().getOrientation() << std::endl;
			of.close();
		}
		BodyIterator::BodyIterator( World * world ){ pWorld = world; pCur = pWorld->getFirstBody(); }
		BodyIterator::BodyIterator( const BodyIterator& rk ){ pWorld = rk.pWorld; pCur = rk.pCur; }
		BodyIterator::~BodyIterator(){}
		
		World * BodyIterator::getWorld() const{ return pWorld; }
		Body * BodyIterator::getCurrent(){ return pCur; }
		Body * BodyIterator::getNext(){ pCur = pWorld->getNextBody( pCur ); return pCur; }

		BodyConstIterator::BodyConstIterator( const World * world ){ pWorld = world; pCur = pWorld->getFirstBody(); }
		BodyConstIterator::BodyConstIterator( const BodyConstIterator& rk ){ pWorld = rk.pWorld; pCur = rk.pCur; }
		BodyConstIterator::~BodyConstIterator(){}
		
		const World * BodyConstIterator::getWorld() const{ return pWorld; }
		const Body * BodyConstIterator::getCurrent(){ return pCur; }
		const Body * BodyConstIterator::getNext(){ pCur = pWorld->getNextBody( pCur );return pCur;  }
		
		Body::Body( World * world, Lit::Real mass , Lit::Entity * ent, NewtonBody * body, CollisionShape* shape )
		{
			mPolygonIterator = boost::bind( &Body::_newtonPerPolyDummy, _1, _2, _3, _4 );
			mWorld = world;
			mShape = shape;
			mMass = mass;
			mEntity = ent;
			mBody = body;
			mStatic = false;
			mInitialState = new BodyState();
		}
		Body::~Body(){ _releaseCollision( false	); delete mInitialState, mInitialState = NULL; }
		Body::PolygonIteratorType Body::mPolygonIterator;
		
		void Body::setMaterialName( const std::string &name ){ mEntity->setMaterialName(name); }
		void Body::setMaterial( Ogre::MaterialPtr mat ){ mEntity->setMaterial( mat ); }
		std::string Body::getMaterialName(){ if(!mEntity ) return ""; return mEntity->getMaterialName(); }
		Ogre::MaterialPtr Body::getMaterial(){ if(!mEntity ) return Ogre::MaterialPtr(); return Ogre::MaterialPtr(Ogre::MaterialManager::getSingleton().getByName(mEntity->getMaterialName())); }

		void Body::_newtonPerPolyDummy( void * userData, int vert_cnt, const float * faceArray, int fid )
		{
		}
		void Body::_newtonPerPoly( void * userData, int vert_cnt, const float * faceArray, int fid )
		{
			mPolygonIterator(userData, vert_cnt, faceArray, fid);
		}

		// for each collision face
		// user data is this
		void Body::forEachPolygon( PolygonIteratorType itor, const Matrix& matrix )
		{
			mPolygonIterator = itor;
			_CollisionPtr col(NewtonBodyGetCollision(mBody));
			float mat0[4][4];
			NewtonBodyGetMatrix( mBody, &mat0[0][0] );

			if( !col ) return;
			NewtonCollisionForEachPolygonDo( *col, &mat0[0][0], _newtonPerPoly, this );
		}
		
		void Body::setFreeState( bool state ){ NewtonBodySetFreezeState(mBody,state); }
		void Body::setAutoSleep( bool sleep ){ NewtonBodySetAutoSleep(mBody,sleep); }

		bool Body::isAutoSleeping() const{ return NewtonBodyGetAutoSleep( mBody ) == 1; }
		bool Body::isFrozen() const{ return NewtonBodyGetFreezeState( mBody ) == 1; }


		_CollisionPtr Body::getCollision() const{ return NewtonBodyGetCollision( mBody ); }

		bool Body::isStatic() const{ return mStatic; }
		void Body::_setStatic( bool state ){ mStatic = state; }
		/*************************************
		**************************************
		**		Inertial Matrix
		**************************************
		**************************************/
	
		Lit::Vec3		Body::getInertia() const{ return getMassMatrix().inertia; }
		InertialMatrix	Body::getInertialMatrix() const{ return InertialMatrix( getMassMatrix(), getCenterOfMass() ); }
		void			Body::setInertia( const InertialMatrix& matrix )
		{ 
			if( mBody ){
				setMassMatrix( MassMatrix(matrix.mass, matrix.inertia) );
				setCenterOfMass (matrix.origin);
			}
		}

		/*************************************
		**************************************
		**		Transformation Matrix
		**************************************
		**************************************/
		void		Body::setMatrix( Matrix mat ){ if( mBody ){ NewtonBodySetMatrix( mBody, mat.toNewton() ); } }
		Matrix		Body::getMatrix() const{ float mat[16]; if( mBody ){ NewtonBodyGetMatrix( mBody, &mat[0] ); } return Matrix(&mat[0]); }
	
		/*************************************
		**************************************
		**		Mass Matrix
		**************************************
		**************************************/
		void		Body::setMassMatrix( const MassMatrix& matrix ){  if(mBody) NewtonBodySetMassMatrix( mBody, matrix.mass, matrix.scaledInertia().x, matrix.scaledInertia().y, matrix.scaledInertia().z );}
		MassMatrix	Body::getMassMatrix() const{ MassMatrix mat(mMass); if( mBody ) NewtonBodyGetMassMatrix( mBody, &mat.mass, &mat.inertia.x, &mat.inertia.y, &mat.inertia.z ); return mat; }
	
		/*************************************
		**************************************
		**		Position/Orientation
		**************************************
		**************************************/
		Lit::Vec3				Body::getPosition() const{ return getMatrix().getPosition(); }
		Lit::Quat				Body::getOrientation() const{ return getMatrix().getOrientation(); }

		void					Body::setPosition( const Lit::Vec3& p ){ setMatrix( Matrix(p, getOrientation()) ); }
		void					Body::setOrientation(const Lit::Quat& o){ setMatrix( Matrix(getPosition(), o) ); }
	
		/*************************************
		**************************************
		**		Body State Holder
		**************************************
		**************************************/
		void					Body::setInitialState( const BodyState& state ){ *mInitialState = state; }
		BodyState*				Body::getInitialState() const{return mInitialState; }
	
		void					Body::defineInitialState(){ mInitialState->saveState(this); }
		void					Body::restoreInitialState(){ mInitialState->restoreState(this); }

		void					Body::restoreState( BodyState &state ){ state.restoreState( this ); }
		BodyState				Body::captureState() const{ BodyState ret; ret.saveState(this); return ret; }
	
		/*************************************
		**************************************
		**		External Forces
		**************************************
		**************************************/
		
		ForceList& Body::getExternalForces(){return mForces;}
		const ForceList& Body::getExternalForces() const{return mForces;}
		void Body::resetExternalForces( ){ mForces.clear(); }
		void Body::setExternalForces( const Physics::Force& f ){mForces.clear(); mForces.push( f );}
		void Body::addExternalForce( const Physics::Force& f ){mForces.push(f);}
	
		/*************************************
		**************************************
		**		Get/Setters
		**************************************
		**************************************/
		void					Body::_setWorld( World * w ){mWorld = w;}
		const World*			Body::getWorld() const{ return mWorld; }
		World *					Body::getWorld(){ return mWorld; }
		Lit::Entity*			Body::getEntity(){ return mEntity; }
		const Lit::Entity*		Body::getEntity() const{ return mEntity; }

		NewtonBody*				Body::getBody(){ return mBody; }
		const NewtonBody*		Body::getBody() const{ return mBody; }

		const CollisionShape*	Body::getShape() const{ return mShape; }
		CollisionShape*			Body::getShape(){ return mShape; }
		void					Body::setShape(CollisionShape* col){ 
			_releaseCollision(false);
			mShape = col;
			NewtonBodySetCollision( mBody, *col->createShape( mWorld->getWorld() ) ); 
		}
		void Body::_releaseCollision( bool destroy_geometry )
		{
			if( mShape ){ 
				if( destroy_geometry )
				{
					mShape->destroyShape( mWorld->getWorld(), NewtonBodyGetCollision(mBody) );
				}
				delete mShape, mShape = NULL;
			}
		}
		Lit::Real				Body::getMass() const{ return mMass; }
		std::string				Body::getName() const{ return mEntity?mEntity->getName():""; }
		Lit::AABB				Body::getBoundingBox() const{ Lit::Vec3 min, max; if( mBody ) NewtonBodyGetAABB( mBody, &min.x, &max.x ); return Lit::AABB( min, max ); }
	
		void					Body::setCenterOfMass( const Lit::Vec3& com ){	if( mBody )	NewtonBodySetCentreOfMass( mBody, &com.x ); }
		Lit::Vec3				Body::getCenterOfMass() const{Lit::Vec3 ret;if( mBody )	NewtonBodyGetCentreOfMass( mBody, &ret.x );return ret;}
	
	};
};